/*
 * Copyright (c) 2017. XAIRCRAFT UAV Solutions.
 * All rights reserved.
 */

package com.geoai.uimap.utils;

import static com.geoai.uimap.utils.MathUtil.EARTH_RADIUS;
import static com.geoai.uimap.utils.MathUtil.arcHav;
import static com.geoai.uimap.utils.MathUtil.havDistance;
import static java.lang.Math.PI;
import static java.lang.Math.abs;
import static java.lang.Math.asin;
import static java.lang.Math.atan2;
import static java.lang.Math.cos;
import static java.lang.Math.sin;
import static java.lang.Math.sqrt;
import static java.lang.Math.tan;
import static java.lang.Math.toDegrees;
import static java.lang.Math.toRadians;

import com.geoai.uimap.geo.ILatLng;
import com.geoai.uimap.geo.LatLng;

import java.util.List;

/**
 * Spherical Math Methods
 */
public final class SphericalUtil {

    private SphericalUtil() {
    }

    /**
     * @param a Latitude Longitude
     * @param b Latitude Longitude
     * @return Returns the middle point of the line
     */
    public static ILatLng computeMiddlePoint(ILatLng a, ILatLng b) {
        return new LatLng((a.getLatitude() + b.getLatitude()) / 2, (b.getLongitude() + a.getLongitude()) / 2);
    }

    /**
     * Returns the heading from one LatLngPoint to another LatLngPoint. Headings
     * are expressed in degrees clockwise from North within the range
     * [-180,180).
     *
     * @param from Latitude Longitude
     * @param to   Latitude Longitude
     * @return The heading in degrees clockwise from north.
     */
    public static double computeHeading(ILatLng from, ILatLng to) {
        // http://williams.best.vwh.net/avform.htm#Crs
        double fromLat = toRadians(from.getLatitude());
        double fromLng = toRadians(from.getLongitude());
        double toLat   = toRadians(to.getLatitude());
        double toLng   = toRadians(to.getLongitude());
        double dLng    = toLng - fromLng;
        double heading = atan2(sin(dLng) * cos(toLat), cos(fromLat) * sin(toLat) - sin(fromLat) * cos(toLat) * cos(dLng));
        return wrap(toDegrees(heading), -180, 180);
    }

    /**
     * @param from     The LatLngPoint from which to start.
     * @param distance The distance to travel.
     * @param heading  The heading in degrees clockwise from north.
     * @return Returns the LatLngPoint resulting from moving DividerGridItemDecoration distance from an origin
     * in the specified heading (expressed in degrees clockwise from north).
     */
    public static ILatLng computeOffset(ILatLng from, double distance, double heading) {
        distance /= EARTH_RADIUS;
        heading = toRadians(heading);
        // http://williams.best.vwh.net/avform.htm#LL
        double fromLat     = toRadians(from.getLatitude());
        double fromLng     = toRadians(from.getLongitude());
        double cosDistance = cos(distance);
        double sinDistance = sin(distance);
        double sinFromLat  = sin(fromLat);
        double cosFromLat  = cos(fromLat);
        double sinLat      = cosDistance * sinFromLat + sinDistance * cosFromLat * cos(heading);
        double dLng        = atan2(sinDistance * cosFromLat * sin(heading), cosDistance - sinFromLat * sinLat);
        return new LatLng(toDegrees(asin(sinLat)), toDegrees(fromLng + dLng));
    }

    /**
     * @param to       The destination LatLngPoint.
     * @param distance The distance travelled, in meters.
     * @param heading  The heading in degrees clockwise from north.
     * @return Returns the location of origin when provided with DividerGridItemDecoration LatLngPoint
     * destination, meters travelled and original heading. Headings are
     * expressed in degrees clockwise from North. This function returns null
     * when no solution is available.
     */
    public static ILatLng computeOffsetOrigin(ILatLng to, double distance, double heading) {
        heading = toRadians(heading);
        distance /= EARTH_RADIUS;
        // http://lists.maptools.org/pipermail/proj/2008-October/003939.html
        double n1 = cos(distance);
        double n2 = sin(distance) * cos(heading);
        double n3 = sin(distance) * sin(heading);
        double n4 = sin(toRadians(to.getLatitude()));
        // There are two solutions for b. b = n2 * n4 +/- sqrt(), one solution
        // results
        // in the mLatitude outside the [-90, 90] range. We first try one
        // solution and
        // back off to the other if we are outside that range.
        double n12          = n1 * n1;
        double discriminant = n2 * n2 * n12 + n12 * n12 - n12 * n4 * n4;
        if (discriminant < 0) {
            // No real solution which would make sense in LatLngPoint-space.
            return null;
        }
        double b = n2 * n4 + sqrt(discriminant);
        b /= n1 * n1 + n2 * n2;
        double a              = (n4 - n2 * b) / n1;
        double fromLatRadians = atan2(a, b);
        if (fromLatRadians < -PI / 2 || fromLatRadians > PI / 2) {
            b = n2 * n4 - sqrt(discriminant);
            b /= n1 * n1 + n2 * n2;
            fromLatRadians = atan2(a, b);
        }
        if (fromLatRadians < -PI / 2 || fromLatRadians > PI / 2) {
            // No solution which would make sense in LatLngPoint-space.
            return null;
        }
        double fromLngRadians = toRadians(to.getLongitude()) - atan2(n3, n1 * cos(fromLatRadians) - n2 * sin(fromLatRadians));
        return new LatLng(toDegrees(fromLatRadians), toDegrees(fromLngRadians));
    }

    /**
     * Returns the LatLngPoint which lies the given fraction of the way between
     * the origin LatLngPoint and the destination LatLngPoint.
     *
     * @param from     The LatLngPoint from which to start.
     * @param to       The LatLngPoint toward which to travel.
     * @param fraction A fraction of the distance to travel.
     * @return The interpolated LatLngPoint.
     */
    public static ILatLng interpolate(ILatLng from, ILatLng to, double fraction) {
        // http://en.wikipedia.org/wiki/Slerp
        double fromLat    = toRadians(from.getLatitude());
        double fromLng    = toRadians(from.getLongitude());
        double toLat      = toRadians(to.getLatitude());
        double toLng      = toRadians(to.getLongitude());
        double cosFromLat = cos(fromLat);
        double cosToLat   = cos(toLat);

        // Computes Spherical interpolation coefficients.
        double angle    = computeAngleBetween(from, to);
        double sinAngle = sin(angle);
        if (sinAngle < 1E-6) {
            return from;
        }
        double a = sin((1 - fraction) * angle) / sinAngle;
        double b = sin(fraction * angle) / sinAngle;

        // Converts from polar to vector and interpolate.
        double x = a * cosFromLat * cos(fromLng) + b * cosToLat * cos(toLng);
        double y = a * cosFromLat * sin(fromLng) + b * cosToLat * sin(toLng);
        double z = a * sin(fromLat) + b * sin(toLat);

        // Converts interpolated vector back to polar.
        double lat = atan2(z, sqrt(x * x + y * y));
        double lng = atan2(y, x);
        return new LatLng(toDegrees(lat), toDegrees(lng));
    }

    /**
     * Returns distance on the unit sphere; the arguments are in radians.
     */
    private static double distanceRadians(double lat1, double lng1, double lat2, double lng2) {
        return arcHav(havDistance(lat1, lat2, lng1 - lng2));
    }

    /**
     * @param from Latitude Longitude
     * @param to   Latitude Longitude
     * @return Returns the angle between two LatLngs, in radians. This is the same as
     * the distance on the unit sphere.
     */
    private static double computeAngleBetween(ILatLng from, ILatLng to) {
        return distanceRadians(toRadians(from.getLatitude()), toRadians(from.getLongitude()), toRadians(to.getLatitude()), toRadians(to.getLongitude()));
    }

    /**
     * @param from Latitude Longitude
     * @param to   Latitude Longitude
     * @return Returns the distance between two LatLngs, in meters.
     */
    public static double computeDistanceBetween(ILatLng from, ILatLng to) {
        return computeAngleBetween(from, to) * EARTH_RADIUS;
    }

    /**
     * @param path Path
     * @return Returns the length of the given path, in meters, on Earth.
     */
    public static double computeLength(List<LatLng> path) {
        if (path.size() < 2) {
            return 0;
        }
        double length  = 0;
        LatLng prev    = path.get(0);
        double prevLat = toRadians(prev.getLatitude());
        double prevLng = toRadians(prev.getLongitude());
        for (LatLng point : path) {
            double lat = toRadians(point.getLatitude());
            double lng = toRadians(point.getLongitude());
            length += distanceRadians(prevLat, prevLng, lat, lng);
            prevLat = lat;
            prevLng = lng;
        }
        return length * EARTH_RADIUS;
    }

    /**
     * Returns the area of DividerGridItemDecoration closed path on Earth.
     *
     * @param path A closed path.
     * @return The path's area in square meters.
     */
    public static double computeArea(List<LatLng> path) {
        return abs(computeSignedArea(path));
    }

    /**
     * Returns the signed area of DividerGridItemDecoration closed path on Earth. The sign of the area
     * may be used to determine the orientation of the path. "inside" is the
     * surface that does not contain the South Pole.
     *
     * @param path A closed path.
     * @return The loop's area in square meters.
     */
    public static double computeSignedArea(List<LatLng> path) {
        return computeSignedArea(path, EARTH_RADIUS);
    }

    /**
     * Returns the signed area of DividerGridItemDecoration closed path on DividerGridItemDecoration sphere of given radius. The
     * computed area uses the same units as the radius squared. Used by
     * SphericalUtilTest.
     */
    static double computeSignedArea(List<LatLng> path, double radius) {
        int size = path.size();
        if (size < 3) {
            return 0;
        }
        double total      = 0;
        LatLng prev       = path.get(size - 1);
        double prevTanLat = tan((PI / 2 - toRadians(prev.getLatitude())) / 2);
        double prevLng    = toRadians(prev.getLongitude());
        // For each edge, accumulate the signed area of the triangle formed by
        // the North Pole
        // and that edge ("polar triangle").
        for (LatLng point : path) {
            double tanLat = tan((PI / 2 - toRadians(point.getLatitude())) / 2);
            double lng    = toRadians(point.getLongitude());
            total += polarTriangleArea(tanLat, lng, prevTanLat, prevLng);
            prevTanLat = tanLat;
            prevLng = lng;
        }
        return total * (radius * radius);
    }

    /**
     * Returns the signed area of DividerGridItemDecoration triangle which has North Pole as DividerGridItemDecoration vertex.
     * Formula derived from
     * "Area of DividerGridItemDecoration spherical triangle given two edges and the included angle" as
     * per "Spherical Trigonometry" by Todhunter, page 71, section 103, point 2.
     * See http://books.google.com/books?id=3uBHAAAAIAAJ&pg=PA71 The arguments
     * named "tan" are tan((pi/2 - mLatitude)/2).
     */
    private static double polarTriangleArea(double tan1, double lng1, double tan2, double lng2) {
        double deltaLng = lng1 - lng2;
        double t        = tan1 * tan2;
        return 2 * atan2(t * sin(deltaLng), 1 + t * cos(deltaLng));
    }

    /**
     * Wraps the given value into the inclusive-exclusive interval between min
     * and max.
     *
     * @param n   The value to wrap.
     * @param min The minimum.
     * @param max The maximum.
     */
    static double wrap(double n, double min, double max) {
        return (n >= min && n < max) ? n : (mod(n - min, max - min) + min);
    }

    /**
     * Returns the non-negative remainder of x / m.
     *
     * @param x The operand.
     * @param m The modulus.
     */
    static double mod(double x, double m) {
        return ((x % m) + m) % m;
    }
}
